%  Goal is 20 meters north of robot.  Robot goes there. Obstacle not in
%  way. 
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[20000];
goalposition = [0,20];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[1];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=0;
evasivegoal=0;


robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[30];
goalposition = [0,20];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[1];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=0;
evasivegoal=0;
%Goal moves in straight line- obstacle follows it
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[20000];
goalposition = [0,20];% need to read this from an external input
robomag=1.5; % command robot to move at this speed
goalspd=1; % integrate this with goal xpd yspd
obstaclespd=[1];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;


%stationary goal >> one moving obstacle

%crossleft
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[-5]; %next desired obstacle x y position
obstacleY=[10];
goalposition = [0,30];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=0;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;

%crossright
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[5]; %next desired obstacle x y position
obstacleY=[10];
goalposition = [0,30];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;

%headon
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[20];
goalposition = [0,30];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=-pi/2;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;


%overtake
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[5];
goalposition = [0,40];% need to read this from an external input
robomag=1.5; % command robot to move at this speed
goalspd=0; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;

%% evasive goal
%overtake
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[5];
goalposition = [0,40];% need to read this from an external input
robomag=1.5; % command robot to move at this speed
goalspd=.5; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi/2;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=1;

%crossright
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[10]; %next desired obstacle x y position
obstacleY=[10];
goalposition = [10,20];% need to read this from an external input
robomag=1.5; % command robot to move at this speed
goalspd=.5; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=pi;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=1;

%crossleft
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[10];
goalposition = [0,30];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=.5; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=0;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;


%headon
robotX=0; %next desired robot x y position
robotY=0;
obstacleX=[0]; %next desired obstacle x y position
obstacleY=[15];
goalposition = [0,30];% need to read this from an external input
robomag=1; % command robot to move at this speed
goalspd=.5; % integrate this with goal xpd yspd
obstaclespd=[.5];    %need to provide to command the obstacle to move at this speed
% 
obstacleangle=-pi/2;
velocityangle=pi/2; %commentout
goalheading=pi/4;
evasivegoal=0;



